#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/image.hpp"
#include <chrono>
#include "opencv2/opencv.hpp"
#include "cv_bridge/cv_bridge.h"

using namespace std::chrono_literals;

// 自定义节点类
class Camera_Test : public rclcpp::Node
{
public:
  Camera_Test() : Node("camera_test_node_cpp")
  {
    // 创建消息订阅者，从摄像头节点订阅图像
    sub_ = this->create_subscription<sensor_msgs::msg::Image>(
        "/image_raw", 10, std::bind(&Camera_Test::image_callback, this, std::placeholders::_1));
    image_pub_ = this->create_publisher<sensor_msgs::msg::Image>("/image_out", 10);
    timer_ = this->create_wall_timer(0.1s, std::bind(&Camera_Test::on_timer, this));
  }

private:
  // 从摄像头节点订阅图像
  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr sub_;
  // 发布图像
  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr image_pub_;
  rclcpp::TimerBase::SharedPtr timer_;
  // 存储图像消息的成员变量
  sensor_msgs::msg::Image message;

  // 图片处理
  void image_callback(const sensor_msgs::msg::Image::SharedPtr img_msg)
  {
    message = *img_msg; // 直接复制整个消息
  }
  void on_timer()
  {
    image_pub_->publish(message); // 发布存储的图像消息
  }
};

int main(int argc, char const *argv[])
{
  // 初始化ROS2客户端
  rclcpp::init(argc, argv);
  // 调用spin函数，并传入节点对象指针
  rclcpp::spin(std::make_shared<Camera_Test>());
  // 资源释放
  rclcpp::shutdown();
  return 0;
}